/* EasyCODE V8 */
/* EasyCODE ( 0
dsESC Bram */
/* EasyCODE ( 0
Includes */
#include <p33FJ16GS504.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "init.h"
#include "statechanges.h"
#include "musica.h"
#include "../../dsESC_Bootloader.X/src/bootloader_init.h"

/* EasyCODE ) */
/* EasyCODE ( 0
Config & Vars */
/* EasyCODE C */

    void InitINT(void);
    void InitADC(void);
    void InitDAC(void);
    void InitPWM(void);
    void bootdelay();
/* Configuration Bit Settings */

_FOSCSEL(FNOSC_PRIPLL)
_FOSC(POSCMD_XT & OSCIOFNC_ON & FCKSM_CSECME)
_FPOR(FPWRT_PWR128 & FWDTEN_OFF)
_FICD(ICS_PGD3 & JTAGEN_OFF)
_FWDT(FWDTEN_OFF)




/* EasyCODE E */
struct setup_t setup;
struct state_t state;
struct pid_t pid;
struct bode_t bode;

const struct setupmeta_t setupmeta[] = {
    {0, "P", &setup.PID_KP,1000},
    {1, "I", &setup.PID_KI,1000},
    {2, "Poles", &setup.MOTOR_POLES,14},
    {3, "RPMmin", &setup.MOTOR_MIN_RPM,500},
    {4, "RPMmax", &setup.MOTOR_MAX_RPM,6500},
    {5, "CurLim", &setup.MOTOR_CURRENT_LIMIT,10},
    {6, "ErrLimit", &setup.errortreshold,10},
    {7, "StaPow", &setup.OPENLOOP_POWER_START,0},
    {8, "StaRPM", &setup.OPENLOOP_RPM_START,500},
    {9, "StaSpe", &setup.OPENLOOP_SPEED_START,60000},
    {10, "StaZC", &setup.OPENLOOP_ZC,24},
    {11, "Tim", &setup.TIMING_DEGREES,20},
    {12, "CHz", &setup.CONTROL_LOOP,1000},
    {13, "FB1", &setup.FEEDBACK_VAR_1,1},
    {14, "FB2", &setup.FEEDBACK_VAR_2,2},
    {15, "FB3", &setup.FEEDBACK_VAR_3,3},
    {16, "I2Cerr", &setup.TIMEOUT,100},
    {17, "PIDerrlim", &setup.PID_LIMIT_ERROR,1000},
    {18, "PWMhz", &setup.PWM_FREQ, 40000},
    {19, "BEOn%%", &setup.BEMF_ON_PROM, 420},
    {20, "BEOff%%", &setup.BEMF_OFF_PROM, 250},
    {21, "A_BEoff%%", &setup.SMP_BEMF_OFF, 800},
    {22, "A_BEon%%", &setup.SMP_BEMF_ON, 5},
    {23, "A_BEonLim", &setup.SMP_BEMF_ON_LIM, 750},
    {24, "PWMmin", &setup.PWM_MIN, 21},
    {25, "PWMmax", &setup.PWM_MAX, 975},
    {26, "DT(ns)", &setup.PWM_DEADTIME, 200},
    {27, "BusLow", &setup.BUS_LOW, 10}
};

/* EasyCODE C */
/* EasyCODE E */


unsigned int PIDStartCounter;
/* EasyCODE C */
/* EasyCODE E */

void PID_loop(void);
/* EasyCODE ) */
/* EasyCODE ( 0
UART Debug functions
 */
/* EasyCODE F */


void loadSetup()
{
    uReg32 reg, val;
    reg.Val32 = 0x200;

    int i;
    for (i=0;i<sizeof(setupmeta)/sizeof(struct setupmeta_t);i++)
    {
        val = boot_getflashword(reg.Val32);

        if (val.Val[2] == ((0xFF ^ val.Val[0]) ^ val.Val[1]))
        {
            *setupmeta[i].pointer = val.Word.LW;
        }
        else
        {
            *setupmeta[i].pointer = setupmeta[i].def;
        }
        reg.Val32 += 2;
    }
}

void saveSetup()
{
    //if (state.mode != STATE_OFF && state.mode != STATE_CONFIG) return;
    unsigned char page[128*3];
    int i;

    uReg32 reg, val;

    for (i=0;i<128;i++)
    {
        reg.Val32 = i*2;
        val = boot_getflashword(reg.Val32);
        page[i*3+0] = val.Val[0];
        page[i*3+1] = val.Val[1];
        page[i*3+2] = val.Val[2];
    }

    boot_erasepage(0);

    for (i=0;i<128;i++)
    {
        reg.Val32 = i * 2;
        val.Val[0] = page[i*3+0];
        boot_setflashword(reg.Val32, val.Val32);
    }

    reg.Val32 = 0x200;

    for (i=0;i<sizeof(setupmeta)/sizeof(struct setupmeta_t);i++)
    {
        val.Word.LW = *setupmeta[i].pointer;
        val.Val[2] =((0xFF ^ val.Val[0]) ^ val.Val[1]);
        boot_setflashword(reg.Val32, val.Val32);
        reg.Val32 += 2;
    }

    boot_writerow();
}

void configUpdateControlLoop()
{
    PR1 = (FCY / 8 / setup.CONTROL_LOOP);
}

void configUpdateCurrentLimit()
{
    if (setup.MOTOR_CURRENT_LIMIT == 0)
        CMPDAC4bits.CMREF = 0x3FF;
    else
        CMPDAC4bits.CMREF=((unsigned int)setup.MOTOR_CURRENT_LIMIT*1000)/117; // = (amps*0.01) / (1.2/1024)
}

void configUpdateTimingDegrees()
{
    const char lookupNum[] = {1,29,7,9,13,5,2,23,11,7,1,19,3,17,4,1,7,13,1,11,1,3,2,7,1,1,1,1,1,1};
    const char lookupDenom[] = {2,60,15,20,30,12,5,60,30,20,3,60,10,60,15,4,30,60,5,60,6,20,15,60,10,12,15,20,30,60};
    if (setup.TIMING_DEGREES >= 30)
    {
        state.delay_num = 0;
    }
    else
    {
        state.delay_num = lookupNum[setup.TIMING_DEGREES];
        state.delay_denom = lookupDenom[setup.TIMING_DEGREES];
    }
}

unsigned int configUpdatePWMGetVal(unsigned int v)
{
    return (long)(setup.calcPWM_FREQ) * (long)v / 1000;
}

void configUpdatePWM()
{
    PTCONbits.PTEN = 0;

    int deadtime;

    if (setup.PWM_FREQ < 10000)
    {
        PTCON2bits.PCLKDIV = 2;
        setup.calcPWM_FREQ = (7490000UL * 16) * 8 / 4 / setup.PWM_FREQ - 8;
        deadtime = setup.PWM_DEADTIME * 100 / 416; //416 / 100;
    }
    else if (setup.PWM_FREQ < 20000)
    {
        PTCON2bits.PCLKDIV = 1;
        setup.calcPWM_FREQ = (7490000UL * 16) * 8 / 2 / setup.PWM_FREQ - 8;
        deadtime = setup.PWM_DEADTIME * 100 / 208;
    }
    else
    {
        PTCON2bits.PCLKDIV = 0;
        setup.calcPWM_FREQ = (7490000UL * 16) * 8 / setup.PWM_FREQ - 8;
        deadtime = setup.PWM_DEADTIME * 100 / 104;
    }

    PTPER = setup.calcPWM_FREQ;

    DTR1=deadtime; //180; //200nS deadtime
    DTR2=deadtime;
    DTR3=deadtime;

    ALTDTR1=deadtime;
    ALTDTR2=deadtime;
    ALTDTR3=deadtime;

    setup.calcPWM_MIN = configUpdatePWMGetVal(setup.PWM_MIN);
    setup.calcPWM_MAX = configUpdatePWMGetVal(setup.PWM_MAX);
    setup.calcBEMF_OFF_PROM = configUpdatePWMGetVal(setup.BEMF_OFF_PROM);
    setup.calcBEMF_ON_PROM = configUpdatePWMGetVal(setup.BEMF_ON_PROM);
    setup.calcSMP_BEMF_OFF = configUpdatePWMGetVal(setup.SMP_BEMF_OFF);
    setup.calcSMP_BEMF_ON = configUpdatePWMGetVal(setup.SMP_BEMF_ON);
    setup.calcSMP_BEMF_ON_LIM = configUpdatePWMGetVal(setup.SMP_BEMF_ON_LIM);

    PTCONbits.PTEN = 1;
}


void setBodeSignal(int injection)
{
    if (state.armed == 0) return;

    bode.hasSignalToInject = 1;
    bode.signalToInject = injection;
}

void setRPM(int rpm)
{
    state.timeoutCount = 0;
    if (state.armed == 0) return;

    if ((rpm < 0 && state.direction == 0) || (rpm > 0 && state.direction == 1))
    {
        if (state.mode == STATE_OFF)
        {
            if (rpm < 0) state.direction = 1;
            else state.direction = 0;

            extern unsigned int PWM_CON_STATES[];
            IOCON1 = PWM_CON_STATES[2];
            IOCON2 = PWM_CON_STATES[4];
            IOCON3 = PWM_CON_STATES[2];

            MDC = 1000;
            //bootdelay();
            IOCON1 = PWM_CON_STATES[0];
            IOCON2 = PWM_CON_STATES[0];
            IOCON3 = PWM_CON_STATES[0];
        }
        else
        {
            rpm = 0;
        }
    }
    if (rpm < 0) rpm = -rpm;

    if (rpm < setup.MOTOR_MIN_RPM) rpm = 0;
    if (rpm > setup.MOTOR_MAX_RPM) rpm = setup.MOTOR_MAX_RPM;

    if (state.mode == STATE_OFF && rpm > 0)
    {
        stateAlignEnter();
    }
    if (state.mode != STATE_OFF && rpm < setup.MOTOR_MIN_RPM)
    {
        stateStoppingEnter();
        rpm = 0;
    }

    pid.target_rpm = rpm;
}

/* EasyCODE ) */
/* EasyCODE ( 0
MAIN */
/* EasyCODE F */
void bootdelay()
{
    T2CONbits.TON = 1;
    T2CONbits.TCKPS = 2;
    TMR2 = 0xA000;
    while (TMR2 != 0);
    T2CONbits.TCKPS = 1;
}

int main(void)
{
    //_POR=0;
    //_BOR=0;

//#ifdef _DEBUG
    boot_initport();
    boot_initclock();
    boot_init_comm();
//#endif
    InitPWM();
    InitADC();
    InitDAC();
    InitINT();

    // Enter off mode
    stateOffEnter();

    WLED = 0;
    GLED = 0;
    BLED = 0;

    IEC0bits.U1RXIE = 1;
    T2CONbits.TON=1;
    PTCONbits.PTEN=1;

    bootdelay();
    motorbeepstop();

    loadSetup();

    configUpdateControlLoop();
    configUpdateCurrentLimit();
    configUpdateTimingDegrees();
    configUpdatePWM();

    stateOffEnter();

    _U1RXIE=1;
    _U1RXIF=0;
    T1CONbits.TON=1;
    T3CONbits.TON=1;
    BLED = 0;
    WLED = 0;
    GLED = 0;
    //IEC1bits.SI2C1IE = 1;
    //while(1) { asm("NOP"); }
    while (1)
    {
        //BLED = state.BEMFmode;
        //IEC0bits.T1IE = 0;
        if (state.armed == 0)
        {
            while (state.mode != STATE_OFF);

            motorbeepstart();
            motorbeep(KEY_A5);
            bootdelay();
            motorbeepstop();
            motorbeepstart();
            motorbeep(KEY_A4);
            bootdelay();
            motorbeepstop();

            while (state.armed == 0)
            {
            motorbeepstart();
            motorbeep(KEY_A5);
            bootdelay();
            motorbeepstop();
            bootdelay();
            bootdelay();
            }

            motorbeepstart();
            motorbeep(KEY_A4);
            bootdelay();
            motorbeepstop();
            motorbeepstart();
            motorbeep(KEY_A5);
            bootdelay();
            motorbeepstop();
        }
        IEC0bits.T1IE = 1;
    }
}

/* EasyCODE ) */
/* EasyCODE ( 0
I2C ISR */
/* EasyCODE F */
unsigned int I2Cstate = 0;

typedef enum {
    I2C_RPM_WRITE,
    I2C_RPM_READ,
    I2C_BODE_WRITE,
    I2C_BODE_READ,
    I2C_CMD_WRITE,
    I2C_CMD_READ
} I2C_TYPE;

I2C_TYPE I2Ctype;
unsigned char I2Cbuffer[4];

void parseReadCommand()
{
    if (I2Cbuffer[0] == 0x01)
    {
        int i, j;
        int bc = I2Cstate;

        for (i=0;i<sizeof(setupmeta)/sizeof(struct setupmeta_t);i++)
        {
            if (bc-- == 0)
            {
                I2CTRN = i;
                return;
            }
            for (j=0;j<1000;j++)
            {
                if (bc-- == 0)
                {
                    I2CTRN = setupmeta[i].name[j];
                    return;
                }
                if (setupmeta[i].name[j] == 0) break;
            }
            if (bc-- == 0)
            {
                I2CTRN = (*setupmeta[i].pointer) >> 8;
                return;
            }
            if (bc-- == 0)
            {
                I2CTRN = (*setupmeta[i].pointer) & 0xFF;
                return;
            }
        }
        I2CTRN = 0xFF;
    }
    else if (I2Cbuffer[0] == 0x30)
    {
        I2C1TRN = state.armed == 0 ? 0x00 : 0x01;
    }
    else if (I2Cbuffer[0] == 0xF0)
    {
        if (I2Cstate == 0) I2C1TRN = 0x00;
        else if (I2Cstate < 3)
        {
            uReg32 val = boot_getflashword(0x800);
            I2C1TRN = val.Val[2-I2Cstate];
        }
        else if (I2Cstate < 5)
        {
            uReg32 val = boot_getflashword(0xFF0000);
            I2C1TRN = val.Val[4-I2Cstate];
        }
        else if (I2Cstate < 7)
        {
            uReg32 val = boot_getflashword(0xFF0002);
            I2C1TRN = val.Val[6-I2Cstate];
        }
        else
            I2C1TRN = 0xFF;
    }
    else if (I2Cbuffer[0] == 0xFE) // bootloader on/off
    {
        I2CTRN = 0x00;
    }
    else
        I2CTRN = 0xFF;
}

void parseWriteCommand()
{
    if (I2Cstate == 4 && I2Cbuffer[0] == 0x02)
    {
        if (I2Cbuffer[1] >= sizeof(setupmeta) / sizeof(struct setupmeta_t))
        {
            return;
        }
        *setupmeta[I2Cbuffer[1]].pointer = (I2Cbuffer[2] << 8) | (I2Cbuffer[3]);

        if (setupmeta[I2Cbuffer[1]].pointer == &setup.TIMING_DEGREES)
            configUpdateTimingDegrees();
        else if (setupmeta[I2Cbuffer[1]].pointer == &setup.MOTOR_CURRENT_LIMIT)
            configUpdateCurrentLimit();
        else if (setupmeta[I2Cbuffer[1]].pointer == &setup.CONTROL_LOOP)
            configUpdateControlLoop();
    }
    else if (I2Cstate == 1 && I2Cbuffer[0] == 0x03)
    {
        saveSetup();
    }
    else if (I2Cstate == 1 && I2Cbuffer[0] == 0xFB)
    {
        asm("NOP");
        asm("NOP");
        asm("RESET");
        while(1);
    }
    else if (I2Cstate == 2 && I2Cbuffer[0] == 0x30)
    {
        if (I2Cbuffer[1] == 1 && state.armed == 0)
        {
            state.armed = 1;
        }
        else if (I2Cbuffer[1] == 0 && state.armed == 1)
        {
            state.armed = 0;
            stateStoppingEnter();
        }
    }
}

int getVar(unsigned char v)
{
    extern struct ADC_t ADC;
    switch (v)
    {
        case 1: return state.direction == 0 ? state.RPM : -state.RPM;
        case 2: return pid.p_value;
        case 3: return pid.i_value;
        case 4: return pid.output;
        case 5: return pid.target_error;
        case 6: return state.RPMpulse;
        case 7: return state.step;
        case 8: return ADC.CURRENT;
        case 9: return ADC.BUS;
        case 10: return ADC.MA;
        case 11: return ADC.MB;
        case 12: return ADC.MC;
        case 13: return MDC;
        case 14: return PR3;
        case 15: return T3CON;
        default: return 0;
    }
}

void __attribute__((interrupt, no_auto_psv))  _SI2C1Interrupt(void)
{
    _SI2C1IF=0;

    if (I2C1STATbits.I2COV == 1)
    {
        I2C1CONbits.I2CEN = 0;
        asm("NOP");
        I2C1CONbits.I2CEN = 1;
        I2C1STATbits.I2COV = 0;
        return;
    }

    // address byte
    if (I2C1STATbits.D_A == 0)
    {
        I2Cstate = 0;
        if ((I2C1RCV & 0x80) == 0x80) I2Ctype = I2C_CMD_WRITE;
        else
        {
            if ((I2C1RCV & 0x40) == 0x40) I2Ctype = I2C_BODE_WRITE;
            else I2Ctype = I2C_RPM_WRITE;
        }

        // write address
        if (I2C1STATbits.R_W == 1)
        {
            I2Ctype++;
        }
        else
        {
            I2C1CONbits.SCLREL = 1;

            // address byte == write, next byte will trigger ISR again..
            return;
        }

        // address byte == read, should write byte to TRN
    }

    // set RPM
    if (I2Ctype == I2C_RPM_WRITE || I2Ctype == I2C_BODE_WRITE)
    {
        if (I2Cstate == 0)
        {
            I2Cbuffer[0] = I2C1RCV;
        }
        else if (I2Cstate == 1)
        {
            if (I2Ctype == I2C_RPM_WRITE)
                setRPM((I2Cbuffer[0] << 8) | I2C1RCV);
            else
                setBodeSignal((I2Cbuffer[0] << 8) | I2C1RCV);
        }

        I2Cstate++;
    }
    // read RPM
    else if (I2Ctype == I2C_RPM_READ)
    {
        if (I2Cstate == 0)
            I2C1TRN = (getVar(setup.FEEDBACK_VAR_1)) >> 8;
        else if (I2Cstate == 1)
            I2C1TRN = (getVar(setup.FEEDBACK_VAR_1)) & 0xFF;
        else if (I2Cstate == 2)
            I2C1TRN = (getVar(setup.FEEDBACK_VAR_2)) >> 8;
        else if (I2Cstate == 3)
            I2C1TRN = (getVar(setup.FEEDBACK_VAR_2)) & 0xFF;
        else if (I2Cstate == 4)
            I2C1TRN = (getVar(setup.FEEDBACK_VAR_3)) >> 8;
        else if (I2Cstate == 5)
            I2C1TRN = (getVar(setup.FEEDBACK_VAR_3)) & 0xFF;
        I2Cstate++;
    }
    else if (I2Ctype == I2C_BODE_READ)
    {
        if (I2Cstate == 0)
            I2C1TRN = bode.hasFeedback | (bode.missedSignal << 1);
        else if (I2Cstate == 1)
            I2C1TRN = bode.feedbackError >> 8;
        else if (I2Cstate == 2)
            I2C1TRN = bode.feedbackError & 0xFF;
        else if (I2Cstate == 3)
            I2C1TRN = bode.feedbackPID >> 8;
        else if (I2Cstate == 4)
            I2C1TRN = bode.feedbackPID & 0xFF;
        else if (I2Cstate == 5)
            I2C1TRN = bode.feedbackSignal >> 8;
        else if (I2Cstate == 6)
            I2C1TRN = bode.feedbackSignal & 0xFF;
        I2Cstate++;
    }
    // write status
    else if (I2Ctype == I2C_CMD_WRITE)
    {
        if (I2Cstate == 4) I2Cstate = 0;

        I2Cbuffer[I2Cstate] = I2C1RCV;
        I2Cstate++;

        parseWriteCommand();
    }
    // read status
    else if (I2Ctype == I2C_CMD_READ)
    {
        parseReadCommand();
        I2Cstate++;
    }
    I2C1CONbits.SCLREL = 1;
}

/* EasyCODE ) */
/* EasyCODE ( 0
TIMER1 ISR @ 500hz */
/* EasyCODE F */

/* EasyCODE ) */
/* EasyCODE ( 0
UART1RX ISR */
/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _U1RXInterrupt(void)
{
    _U1RXIF=0;
}

/* EasyCODE ) */
/* EasyCODE ( 0
ERROR TRAPS */
/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _OscillatorFail(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}

/* EasyCODE F */
void (*getErrLoc(void))(void);  // Get Address Error Loc
unsigned int errLoc;           // Function Pointer
void __attribute__((interrupt, no_auto_psv)) _AddressError(void)
{
    errLoc = (unsigned int)getErrLoc();
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}

/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _StackError(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}

/* EasyCODE F */
void __attribute__((interrupt, no_auto_psv)) _MathError(void)
{
    #ifdef __DEBUG
        while (1)
        {
            asm("NOP");
        }
    #endif
    asm("RESET");
}




void i2cPulse()
{
    I2C1CONbits.I2CEN = 0;
    ODCBbits.ODCB7 = 1;
    PORTBbits.RB7 = 0;
    TRISBbits.TRISB7 = 0;
    asm("NOP");
    asm("NOP");
    asm("NOP");
    asm("NOP");
    TRISBbits.TRISB7 = 1;
    I2C1CONbits.I2CEN = 1;
}

void PID_loop(void)
{
    extern struct setup_t setup;
    extern struct state_t state;
    extern struct pid_t pid;
    extern struct bode_t bode;

    if (state.mode > STATE_ALIGN && state.rampup < setup.OPENLOOP_ZC)
        pid.target_rpm = setup.OPENLOOP_RPM_START;

    if (pid.target_rpm < setup.MOTOR_MIN_RPM)
    {
        pid.output = 0;
        return ;
    }
    if (pid.target_rpm > setup.MOTOR_MAX_RPM)
    {
        pid.target_rpm = setup.MOTOR_MAX_RPM;
    }
    pid.target_error = (pid.target_rpm - state.RPM);
    int PID_LIMIT = setup.PID_LIMIT_ERROR;
    if (pid.target_error > PID_LIMIT)
    {
        pid.target_error = PID_LIMIT;
    }
    else
    {
        if (pid.target_error < -PID_LIMIT)
        {
            pid.target_error = -PID_LIMIT;
        }
    }

    //#define qMul(a,b)           (q_t)   ( ((((long long)(a)*(b)+Q_FRACVAL/2 ))) >> (Q_FRACBITS +1))

    pid.p_value = (((long)pid.target_error*setup.PID_KP+1024) >> 11);

    //P-factor
    // pid.p_value = ((long)pid.target_error*setup.PID_KP/1024);
    //I-factor

    pid.i_value = pid.i_value + (((long)pid.target_error*setup.PID_KI+8192) >> 14);

    //pid.i_value = pid.i_value + ((long)pid.target_error*setup.PID_KI)/16384;

    if (pid.i_value > setup.calcPWM_MAX)
    {
        pid.i_value = setup.calcPWM_MAX;
    }
    if (pid.i_value < 0)
    {
        pid.i_value = 0;
    }
    //PID
    int longPID = pid.p_value + pid.i_value;

    if (state.bodeMode)
    {
        // inject bode signal
        longPID += bode.signalInjected;
    }


    pid.output = pid.target_rpm;
    /*

    if (longPID>setup.calcPWM_MAX)
    {
        pid.output = setup.calcPWM_MAX;
    }
    else
    {
        if (longPID < setup.calcPWM_MIN)
        {
            pid.output = setup.calcPWM_MIN;
        }
        else
        {
            pid.output = longPID;
        }
    }*/
    _PSEMIE = 1;
}


void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void)
{
//    state.startCalculation=1;
    T1CALC_ENTER;
    extern struct setup_t setup;
    extern struct state_t state;
    extern struct pid_t pid;
    extern struct bode_t bode;
    IFS0bits.T1IF = 0; //Clear timer flag for next period

    if (state.mode <= STATE_ALIGN)
    {
        T1CALC_EXIT;
        return;
    }

//        if (PIDStartCounter<(PIDStartDelay/2)) continue;
    unsigned long RPMc;// = RPMpulse;
    RPMc = FCY / state.RPMpulse;
    RPMc *= 60 / 3;
    RPMc /= setup.MOTOR_POLES;
    RPMc /= (unsigned int)1000;
    state.RPM = RPMc; //state.RPM / 2 + RPMc / 2;

    //if (pid.target_rpm < setup.MOTOR_MIN_RPM && state.mode != STATE_OFF && state.RPM < setup.MOTOR_MIN_RPM * 4)
    //    stateOffEnter();

    if (bode.hasSignalToInject)
    {
        if (state.bodeMode == 1)
        {
            bode.feedbackSignal = bode.signalInjected;
            bode.feedbackPID = pid.output - bode.signalInjected;
            bode.feedbackError = pid.target_error;
            bode.hasFeedback = 1;
        }
        else
        {
            bode.missedSignal = 0;
            bode.hasFeedback = 0;
        }

        bode.signalInjected = bode.signalToInject;
        bode.hasSignalToInject = 0;
        state.bodeMode = 1;

        PID_loop();
        i2cPulse();
    }
    else if (state.bodeMode == 1)
    {
        bode.feedbackError = pid.target_error;
        bode.feedbackPID = pid.output - bode.signalInjected;
        bode.feedbackSignal = bode.signalInjected;
        bode.hasFeedback = 1;
        bode.signalInjected = 0;
        PID_loop();
        state.bodeMode = 0;
        bode.missedSignal = 1;
    }
    else
    {
        PID_loop();
        if (state.timeoutCount++ >= setup.TIMEOUT)
        {
            stateStoppingEnter();
        }
    }

    T1CALC_EXIT;
}


/* EasyCODE ) */
/* EasyCODE ) */
